From 6efb946e6cb808c35e4fb620974c12244a95b4e7 Mon Sep 17 00:00:00 2001
From: Naushir Patuck <naush@raspberrypi.com>
Date: Fri, 31 Mar 2023 14:56:09 +0100
Subject: [PATCH] drivers: media: imx708: Increase usable link
 frequencies

Add support for three different usable link frequencies (default 450Mhz,
447Mhz, and 453MHz) for the IMX708 camera sensor. The choice of
frequency is handled thorugh the "link-frequency" overlay parameter.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
---
 drivers/media/i2c/imx708.c | 94 +++++++++++++++++++++++++++++++-------
 1 file changed, 78 insertions(+), 16 deletions(-)

--- a/drivers/media/i2c/imx708.c
+++ b/drivers/media/i2c/imx708.c
@@ -35,8 +35,6 @@
 
 #define IMX708_XCLK_FREQ		24000000
 
-#define IMX708_DEFAULT_LINK_FREQ	450000000
-
 /* Default initial pixel rate, will get updated for each mode. */
 #define IMX708_INITIAL_PIXEL_RATE	590000000
 
@@ -181,6 +179,50 @@ static const u8 pdaf_gains[2][9] = {
 	{ 0x35, 0x35, 0x35, 0x38, 0x3e, 0x46, 0x4c, 0x4c, 0x4c }
 };
 
+/* Link frequency setup */
+enum {
+	IMX708_LINK_FREQ_450MHZ,
+	IMX708_LINK_FREQ_447MHZ,
+	IMX708_LINK_FREQ_453MHZ,
+};
+
+static const s64 link_freqs[] = {
+	[IMX708_LINK_FREQ_450MHZ] = 450000000,
+	[IMX708_LINK_FREQ_447MHZ] = 447000000,
+	[IMX708_LINK_FREQ_453MHZ] = 453000000,
+};
+
+/* 450MHz is the nominal "default" link frequency */
+static const struct imx708_reg link_450Mhz_regs[] = {
+	{0x030E, 0x01},
+	{0x030F, 0x2c},
+};
+
+static const struct imx708_reg link_447Mhz_regs[] = {
+	{0x030E, 0x01},
+	{0x030F, 0x2a},
+};
+
+static const struct imx708_reg link_453Mhz_regs[] = {
+	{0x030E, 0x01},
+	{0x030F, 0x2e},
+};
+
+static const struct imx708_reg_list link_freq_regs[] = {
+	[IMX708_LINK_FREQ_450MHZ] = {
+		.regs = link_450Mhz_regs,
+		.num_of_regs = ARRAY_SIZE(link_450Mhz_regs)
+	},
+	[IMX708_LINK_FREQ_447MHZ] = {
+		.regs = link_447Mhz_regs,
+		.num_of_regs = ARRAY_SIZE(link_447Mhz_regs)
+	},
+	[IMX708_LINK_FREQ_453MHZ] = {
+		.regs = link_453Mhz_regs,
+		.num_of_regs = ARRAY_SIZE(link_453Mhz_regs)
+	},
+};
+
 static const struct imx708_reg mode_common_regs[] = {
 	{0x0100, 0x00},
 	{0x0136, 0x18},
@@ -278,8 +320,6 @@ static const struct imx708_reg mode_4608
 	{0x0307, 0x7C},
 	{0x030B, 0x02},
 	{0x030D, 0x04},
-	{0x030E, 0x01},
-	{0x030F, 0x2C},
 	{0x0310, 0x01},
 	{0x3CA0, 0x00},
 	{0x3CA1, 0x64},
@@ -376,8 +416,6 @@ static const struct imx708_reg mode_2x2b
 	{0x0307, 0x7A},
 	{0x030B, 0x02},
 	{0x030D, 0x04},
-	{0x030E, 0x01},
-	{0x030F, 0x2C},
 	{0x0310, 0x01},
 	{0x3CA0, 0x00},
 	{0x3CA1, 0x3C},
@@ -472,8 +510,6 @@ static const struct imx708_reg mode_2x2b
 	{0x0307, 0x76},
 	{0x030B, 0x02},
 	{0x030D, 0x04},
-	{0x030E, 0x01},
-	{0x030F, 0x2C},
 	{0x0310, 0x01},
 	{0x3CA0, 0x00},
 	{0x3CA1, 0x3C},
@@ -568,8 +604,6 @@ static const struct imx708_reg mode_hdr_
 	{0x0307, 0xA2},
 	{0x030B, 0x02},
 	{0x030D, 0x04},
-	{0x030E, 0x01},
-	{0x030F, 0x2C},
 	{0x0310, 0x01},
 	{0x3CA0, 0x00},
 	{0x3CA1, 0x00},
@@ -795,6 +829,7 @@ struct imx708 {
 	struct v4l2_ctrl *blue_balance;
 	struct v4l2_ctrl *notify_gains;
 	struct v4l2_ctrl *hdr_mode;
+	struct v4l2_ctrl *link_freq;
 
 	/* Current mode */
 	const struct imx708_mode *mode;
@@ -813,6 +848,8 @@ struct imx708 {
 
 	/* Current long exposure factor in use. Set through V4L2_CID_VBLANK */
 	unsigned int long_exp_shift;
+
+	unsigned int link_freq_idx;
 };
 
 static inline struct imx708 *to_imx708(struct v4l2_subdev *_sd)
@@ -1428,7 +1465,7 @@ static int imx708_get_selection(struct v
 static int imx708_start_streaming(struct imx708 *imx708)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&imx708->sd);
-	const struct imx708_reg_list *reg_list;
+	const struct imx708_reg_list *reg_list, *freq_regs;
 	int i, ret;
 	u32 val;
 
@@ -1474,6 +1511,16 @@ static int imx708_start_streaming(struct
 		return ret;
 	}
 
+	/* Update the link frequency registers */
+	freq_regs = &link_freq_regs[imx708->link_freq_idx];
+	ret = imx708_write_regs(imx708, freq_regs->regs,
+				freq_regs->num_of_regs);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set link frequency registers\n",
+			__func__);
+		return ret;
+	}
+
 	/* Apply customized values from user */
 	ret =  __v4l2_ctrl_handler_setup(imx708->sd.ctrl_handler);
 	if (ret)
@@ -1720,6 +1767,7 @@ static int imx708_init_controls(struct i
 	struct v4l2_ctrl_handler *ctrl_hdlr;
 	struct i2c_client *client = v4l2_get_subdevdata(&imx708->sd);
 	struct v4l2_fwnode_device_properties props;
+	struct v4l2_ctrl *ctrl;
 	unsigned int i;
 	int ret;
 
@@ -1738,6 +1786,12 @@ static int imx708_init_controls(struct i
 					       IMX708_INITIAL_PIXEL_RATE, 1,
 					       IMX708_INITIAL_PIXEL_RATE);
 
+	ctrl = v4l2_ctrl_new_int_menu(ctrl_hdlr, &imx708_ctrl_ops,
+				      V4L2_CID_LINK_FREQ, 0, 0,
+				      &link_freqs[imx708->link_freq_idx]);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
 	/*
 	 * Create the controls here, but mode specific limits are setup
 	 * in the imx708_set_framing_limits() call below.
@@ -1833,13 +1887,14 @@ static void imx708_free_controls(struct
 	mutex_destroy(&imx708->mutex);
 }
 
-static int imx708_check_hwcfg(struct device *dev)
+static int imx708_check_hwcfg(struct device *dev, struct imx708 *imx708)
 {
 	struct fwnode_handle *endpoint;
 	struct v4l2_fwnode_endpoint ep_cfg = {
 		.bus_type = V4L2_MBUS_CSI2_DPHY
 	};
 	int ret = -EINVAL;
+	int i;
 
 	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
 	if (!endpoint) {
@@ -1864,11 +1919,18 @@ static int imx708_check_hwcfg(struct dev
 		goto error_out;
 	}
 
-	if (ep_cfg.nr_of_link_frequencies != 1 ||
-	    ep_cfg.link_frequencies[0] != IMX708_DEFAULT_LINK_FREQ) {
+	for (i = 0; i < ARRAY_SIZE(link_freqs); i++) {
+		if (link_freqs[i] == ep_cfg.link_frequencies[0]) {
+			imx708->link_freq_idx = i;
+			break;
+		}
+	}
+
+	if (i == ARRAY_SIZE(link_freqs)) {
 		dev_err(dev, "Link frequency not supported: %lld\n",
 			ep_cfg.link_frequencies[0]);
-		goto error_out;
+			ret = -EINVAL;
+			goto error_out;
 	}
 
 	ret = 0;
@@ -1893,7 +1955,7 @@ static int imx708_probe(struct i2c_clien
 	v4l2_i2c_subdev_init(&imx708->sd, client, &imx708_subdev_ops);
 
 	/* Check the hardware configuration in device tree */
-	if (imx708_check_hwcfg(dev))
+	if (imx708_check_hwcfg(dev, imx708))
 		return -EINVAL;
 
 	/* Get system clock (xclk) */
